#include <rosbag/bag.h>
#include <std_msgs/Int32.h>
#include <ros/ros.h>
#include "altimeter_converter.h"
#include "encoder_converter.h"
#include "fog_converter.h"
#include "gps_converter.h"
#include "vrs_converter.h"
#include "imu_converter.h"
#include "velodyne_converter.h"
#include "sick_converter.h"
#include "stereo_converter.h"

using namespace kaist_publisher;

enum SensorType {
    kAltimeter = 0,
    kEncoder,
    kFog,
    kGps,
    kVrs,
    kImu,
    kVelodyne,
    kStereo,
    // kSick,
    kSensorTypeCount
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "kaist_publisher");

    ros::NodeHandle nh;
    std::vector<bool> sensors(kSensorTypeCount, false);
    std::vector<std::string> sensor_names = {
        "altimeter",
        "encoder",
        "fog",
        "gps",
        "vrs",
        "imu",
        "velodyne",
        "stereo" // ,
        // "sick"
    };

    for (size_t i = 0; i < sensors.size(); ++i) {
        bool sensor_on = false;
        nh.getParam(sensor_names[i], sensor_on);
        sensors[i] = sensor_on;
    }

    std::string dataset;
    nh.getParam("dataset", dataset);
    std::string save_to;
    nh.getParam("save_to", save_to);

    std::string altimeter_topic;
    nh.getParam("altimeter_topic", altimeter_topic);
    AltimeterConverter altimeter(nh, dataset, save_to, altimeter_topic);
    

    std::string encoder_topic;
    nh.getParam("encoder_topic", encoder_topic);
    EncoderConverter encoder(nh, dataset, save_to, encoder_topic);
    

    std::string fog_topic;
    nh.getParam("fog_topic", fog_topic);
    FogConverter fog(nh, dataset, save_to, fog_topic);
    

    std::string gps_topic;
    nh.getParam("gps_topic", gps_topic);
    GpsConverter gps(nh, dataset, save_to, gps_topic);
    

    std::string vrs_topic;
    nh.getParam("vrs_topic", vrs_topic);
    VrsConverter vrs(nh, dataset, save_to, vrs_topic);
    

    std::string irp_topic, raw_topic, mag_topic;
    nh.getParam("imu_irp_topic", irp_topic);
    nh.getParam("imu_raw_topic", raw_topic);
    nh.getParam("imu_mag_topic", mag_topic);
    ImuConverter imu(nh, dataset, save_to, irp_topic, raw_topic, mag_topic);
    

    std::string vlp_left_topic, vlp_right_topic;
    nh.getParam("velodyne_left_topic", vlp_left_topic);
    nh.getParam("velodyne_right_topic", vlp_right_topic);
    VelodyneConverter vlp(nh, dataset, save_to, vlp_left_topic, vlp_right_topic);
    

    std::string stereo_left_topic, stereo_right_topic;
    nh.getParam("stereo_left_topic", stereo_left_topic);
    nh.getParam("stereo_right_topic", stereo_right_topic);
    StereoConverter stereo(nh, dataset, save_to, stereo_left_topic, stereo_right_topic);

    // std::string sick_back_topic, sick_middle_topic;
    // nh.getParam("sick_back_topic", sick_back_topic);
    // nh.getParam("sick_middle_topic", sick_middle_topic);
    // SickConverter sick(nh, dataset, save_to, sick_back_topic, sick_middle_topic);

    int publish_delay;
    nh.getParam("publish_delay", publish_delay);
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;

    std::vector<int64_t> timestamp_sensor(sensors.size(), LONG_MAX);
    if (sensors[0]) 
        timestamp_sensor[0] = altimeter.Init_Publish();
    if (sensors[1]) 
        timestamp_sensor[1] = encoder.Init_Publish();
    if (sensors[2]) 
        timestamp_sensor[2] = fog.Init_Publish();
    if (sensors[3]) 
        timestamp_sensor[3] = gps.Init_Publish();
    if (sensors[4]) 
        timestamp_sensor[4] = vrs.Init_Publish();
    if (sensors[5]) 
        timestamp_sensor[5] = imu.Init_Publish();
    if (sensors[6])
        timestamp_sensor[6] = vlp.Init_Publish();
    if (sensors[7]) 
        timestamp_sensor[7] = stereo.Init_Publish();
    
    ros::Rate r(1000.0 / publish_delay);
    int cur_idx = int(min_element(timestamp_sensor.begin(), timestamp_sensor.end()) - timestamp_sensor.begin());
    while (timestamp_sensor[cur_idx] != LONG_MAX) {
        if (cur_idx == 0) 
            timestamp_sensor[cur_idx] = altimeter.Publish();
        else if (cur_idx == 1) 
            timestamp_sensor[cur_idx] = encoder.Publish();
        else if (cur_idx == 2) 
            timestamp_sensor[cur_idx] = fog.Publish();
        else if (cur_idx == 3) 
            timestamp_sensor[cur_idx] = gps.Publish();
        else if (cur_idx == 4) 
            timestamp_sensor[cur_idx] = vrs.Publish();
        else if (cur_idx == 5) 
            timestamp_sensor[cur_idx] = imu.Publish();
        else if (cur_idx == 6) 
            timestamp_sensor[cur_idx] = vlp.Publish();
        else if (cur_idx == 7) 
            timestamp_sensor[cur_idx] = stereo.Publish();
        
        cur_idx = int(min_element(timestamp_sensor.begin(), timestamp_sensor.end()) - timestamp_sensor.begin());
        r.sleep();
    }
  
    return 0;
}